/*
 * Copyright (c) 2024 iSOFT INFRASTRUCTURE SOFTWARE CO., LTD.
 * easyAda is licensed under Mulan PubL v2.
 * You can use this software according to the terms and conditions of the Mulan PubL v2.
 * You may obtain a copy of Mulan PubL v2 at:
 *          http://license.coscl.org.cn/MulanPubL-2.0
 * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
 * EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
 * MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
 * See the Mulan PubL v2 for more details.
 */

#ifndef ARM64_UTILS_H
#define ARM64_UTILS_H

#include <plat/config.h>

#include <stdlib/assert.h>
#include <stdlib/stdint.h>
#include <tools/macros.h>

#define MIN(a, b) (((a) < (b)) ? (a) : (b))
#define MAX(a, b) (((a) > (b)) ? (a) : (b))
#ifdef CONFIG_ARCH_AARCH64
#define MRS_BIT64_INLINE_FUNCTION(FUNC, REG)                          \
    static inline uint64_t FUNC(void)                                 \
    {                                                                 \
        uint64_t value;                                               \
        __asm__ __volatile__("mrs %0, " #REG "\n\t" : "=r"(value) :); \
        return value;                                                 \
    }

#define MSR_BIT64_INLINE_FUNCTION(FUNC, REG)  \
    static inline void FUNC(uint64_t value)   \
    {                                         \
        __asm__ __volatile__("msr " #REG ", " \
                             "%0"             \
                             "\n\t"           \
                             :                \
                             : "r"(value));   \
    }

#define MRS_BIT32_INLINE_FUNCTION(FUNC, REG)                          \
    static inline uint32_t FUNC(void)                                 \
    {                                                                 \
        uint32_t value;                                               \
        __asm__ __volatile__("mrs %0, " #REG "\n\t" : "=r"(value) :); \
        return value;                                                 \
    }

#define MSR_BIT32_INLINE_FUNCTION(FUNC, REG)  \
    static inline void FUNC(uint32_t value)   \
    {                                         \
        __asm__ __volatile__("msr " #REG ", " \
                             "%0"             \
                             "\n\t"           \
                             :                \
                             : "r"(value));   \
    }

#define VMRS_INLINE_FUNCTION(FUNC, REG)                            \
    static inline uint32_t FUNC(void)                              \
    {                                                              \
        uint32_t value;                                            \
        __asm__ __volatile__("vmrs %0, " #REG "\n" : "=r"(value)); \
        return value;                                              \
    }

#define VMSR_INLINE_FUNCTION(FUNC, REG)                              \
    static inline void FUNC(uint32_t value)                          \
    {                                                                \
        __asm__ __volatile__("vmsr " #REG ", %0 \n" : : "r"(value)); \
    }

#define SCR_BIT64_RW_FUNCTIONS(REG)                \
    MRS_BIT64_INLINE_FUNCTION(scr_read_##REG, REG) \
    MSR_BIT64_INLINE_FUNCTION(scr_write_##REG, REG)

#define SCR_BIT64_RO_FUNCTIONS(REG) MRS_BIT64_INLINE_FUNCTION(scr_read_##REG, REG)

#define SCR_BIT64_WO_FUNCTIONS(REG) MSR_BIT64_INLINE_FUNCTION(scr_write_##REG, REG)

#define SCR_BIT32_RW_FUNCTIONS(REG)                \
    MRS_BIT32_INLINE_FUNCTION(scr_read_##REG, REG) \
    MSR_BIT32_INLINE_FUNCTION(scr_write_##REG, REG)

#define SCR_BIT32_RO_FUNCTIONS(REG) MRS_BIT32_INLINE_FUNCTION(scr_read_##REG, REG)

#define SCR_BIT32_WO_FUNCTIONS(REG) MSR_BIT32_INLINE_FUNCTION(scr_write_##REG, REG)

#define SCR_RW_FUNCTIONS_V(REG)               \
    VMRS_INLINE_FUNCTION(scr_read_##REG, REG) \
    VMSR_INLINE_FUNCTION(scr_write_##REG, REG)

#define SCR_RO_FUNCTIONS_V(REG) VMRS_INLINE_FUNCTION(scr_read_##REG, REG)

#define SCR_WO_FUNCTIONS_V(REG) VMSR_INLINE_FUNCTION(scr_write_##REG, REG)
#else
#define MRC_INLINE_FUNCTION(FUNC, CP, OP1, CRn, CRm, OP2)                                                   \
    static inline uint32_t FUNC(void)                                                                       \
    {                                                                                                       \
        uint32_t value;                                                                                     \
        __asm__ __volatile__("mrc " #CP ", " #OP1 ", %0, " #CRn "," #CRm ", " #OP2 "\n\t" : "=r"(value) :); \
        return value;                                                                                       \
    }

#define MCR_INLINE_FUNCTION(FUNC, CP, OP1, CRn, CRm, OP2)                                                  \
    static inline void FUNC(uint32_t value)                                                                \
    {                                                                                                      \
        __asm__ __volatile__("mcr " #CP ", " #OP1 ", %0, " #CRn "," #CRm ", " #OP2 "\n\t" : : "r"(value)); \
    }

#define SCR_RW_FUNCTIONS32(REG, CP, OP1, CRn, CRm, OP2)         \
    MRC_INLINE_FUNCTION(scr_read_##REG, CP, OP1, CRn, CRm, OP2) \
    MCR_INLINE_FUNCTION(scr_write_##REG, CP, OP1, CRn, CRm, OP2)

#define SCR_RO_FUNCTIONS32(REG, CP, OP1, CRn, CRm, OP2) MRC_INLINE_FUNCTION(scr_read_##REG, CP, OP1, CRn, CRm, OP2)

#define SCR_WO_FUNCTIONS32(REG, CP, OP1, CRn, CRm, OP2) MCR_INLINE_FUNCTION(scr_write_##REG, CP, OP1, CRn, CRm, OP2)
#endif
#define CLZ(val)                                        \
    ({                                                  \
        unsigned long ret = 0;                          \
        __asm__("clz %0, %1;" : "=&r"(ret) : "r"(val)); \
        ret;                                            \
    })
#define GET_ELR_EL2()                                   \
    ({                                                  \
        uint64_t elr_el2;                               \
        asm volatile("mrs %0,elr_el2" : "=r"(elr_el2)); \
        elr_el2;                                        \
    })
#define GET_SPSR_EL2()                                    \
    ({                                                    \
        uint64_t spsr_el2;                                \
        asm volatile("mrs %0,spsr_el2" : "=r"(spsr_el2)); \
        spsr_el2;                                         \
    })
#define GET_SP()                              \
    ({                                        \
        uint64_t sp;                          \
        asm volatile("mov %0,sp" : "=r"(sp)); \
        sp;                                   \
    })
#define GET_X30()                               \
    ({                                          \
        uint64_t x30;                           \
        asm volatile("mov %0,x30" : "=r"(x30)); \
        x30;                                    \
    })
#define GET_X29()                               \
    ({                                          \
        uint64_t x29;                           \
        asm volatile("mov %0,x29" : "=r"(x29)); \
        x29;                                    \
    })

static inline unsigned long clz(unsigned long val)
{
    return CLZ(val);
}

static inline unsigned long biol1(unsigned long val)
{
    return (sizeof(unsigned long) * 8UL) - 1UL - clz(val);
}

static inline unsigned long ln2(unsigned long val)
{
    unsigned long result;

    assert(val != 0);
    result = biol1(val);

    if ((val & ~bitmask(result)) > 0UL) {
        result += 1UL;
    }

    return result;
}

static inline void dsb(void)
{
    __asm__ __volatile__("dsb sy" ::: "memory");
}

static inline void dmb(void)
{
    __asm__ __volatile__("dmb sy" ::: "memory");
}

static inline void isb(void)
{
    __asm__ __volatile__("isb sy" ::: "memory");
}

static inline uint64_t get_elr_el2(void)
{
    return GET_ELR_EL2();
}

static inline uint64_t get_spsr_el2(void)
{
    return GET_SPSR_EL2();
}
static inline uint64_t get_sp(void)
{
    return GET_SP();
}
static inline uint64_t get_x30(void)
{
    return GET_X30();
}
static inline uint64_t get_x29(void)
{
    return GET_X29();
}

#endif
